/*!
  @file validity_checker_impl.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _VALIDITY_CHECKER_IMPL_H_
#define _VALIDITY_CHECKER_IMPL_H_

#include <cmath>

#include <opencv2/core/core.hpp>

#include "mrover_vo/feature_matcher.h"
#include "mrover_vo/validity_checker.h"

namespace vo {

/*! 
 *  @brief  Validity checker for stereo matching
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  This class adds the error check for feature matching. The checkForValidity() 
 *  is called by the FeatureMatcher class to filter invalid point association.
 */

class ValidityCheckerForStereoImages : public ValidityChecker<FeatureMatcher, cv::Point2f> {
 public:
  ValidityCheckerForStereoImages() { }

  ~ValidityCheckerForStereoImages() { }

  virtual bool isValid(const cv::Point2f &input, const cv::Point2f &target) const {
#if 0
    // for Blanco dataset
    return ((std::fabs(input.y - target.y) < 200)
            && (input.x > target.x)
            && (std::fabs(input.x - target.x) > 50)
            );
#else
    // for M6 dataset
    return ((input.y - target.y > 50) && (input.y - target.y < 300)
            && (input.x > target.x)
            && (std::fabs(input.x - target.x) > 10)
            );
#endif
  }
};

/*! 
 *  @brief  Validity checker for tracking
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  This class adds the error check for feature matching. The checkForValidity() 
 *  is called by the FeatureMatcher class to filter invalid point association.
 */

class ValidityCheckerForTracking : public ValidityChecker<FeatureMatcher, cv::Point2f> {
 public:
  ValidityCheckerForTracking() : radius_(200) { }

  ~ValidityCheckerForTracking() { }

  void setRadius(const float radius) { radius_ = radius; }

  //! checks tracking radius constraint
  virtual bool isValid(const cv::Point2f &input, const cv::Point2f &target) const {
    return (cv::norm(input-target) < radius_);
  }

 private:
  float radius_;
};

}  // namespace vo

#endif
